#pragma once

#include <pcl/point_cloud.h>
#include <Eigen/Dense>
#include <rviz_common/visibility_control.hpp>
#include <pcl/point_types.h>
#include <geometry_msgs/msg/quaternion.hpp>
#include <QJsonObject>
#include <QString>


namespace rviz_custom_plugins
{

namespace utils
{


/**
 * @brief 根据点云中的点拟合平面，平面方程可以用ax+by+cz+d=0来代表
 *
 * @param[in] pclPcd 点云
 * @param[out] coefficients 平面方程的四个参数
 * @return 是否成功
 * @note
 * @exception
 */
RVIZ_COMMON_PUBLIC
bool fitPlane(pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclPcd, Eigen::Vector4f& coefficients);



/**
 * @brief 计算点云中的点投影到平面上的对应的点，平面方程可以用ax+by+cz+d=0来代表
 * @param[in] pclPcd 点云
 * @param[in] coefficients 平面方程的四个参数
 * @return 投影点构成的点云
 * @note
 * @exception
 */
RVIZ_COMMON_PUBLIC
pcl::PointCloud<pcl::PointXYZINormal>::Ptr projectPointsToPlane(
    const pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclPcd,
    const Eigen::Vector4f& coefficients);



RVIZ_COMMON_PUBLIC
geometry_msgs::msg::Quaternion eigenMatrix3fToQuaternion(const Eigen::Matrix3f& rot);



RVIZ_COMMON_PUBLIC 
QString pclPointToStr(const pcl::PointXYZINormal& point);



RVIZ_COMMON_PUBLIC 
pcl::PointXYZINormal strToPclPoint(const QString& point);


}


}
